#pragma once

// 各种头文件
// C++标准库
#include <fstream>
#include <vector>
using namespace std;

// OpenCV
#include <opencv2/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
// PCL
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
// yaml-cpp
#include <yaml-cpp/yaml.h>

// 类型定义
using PointT = pcl::PointXYZRGBA;
using PointCloud = pcl::PointCloud<PointT>;

// 相机内参结构
struct CAMERA_INTRINSIC_PARAMETERS
{
    double cx, cy, fx, fy, scale;
};

// 帧结构
struct FRAME
{
    int frameID;
    cv::Mat rgb, depth;      // 该帧对应的彩色图与深度图
    cv::Mat desp;            // 特征描述子
    vector<cv::KeyPoint> kp; // 关键点
};

// PnP 结果
struct RESULT_OF_PNP
{
    cv::Mat rvec, tvec;
    int inliers;
};
class ParameterReader
{
  public:
    YAML::Node config;
    ParameterReader(string filename = "../data/config.yml")
    {
        ifstream fin(filename.c_str());
        if (!fin.is_open())
        {
            cout << "open file " << filename << " failed" << endl;
        }
        config = YAML::Load(fin);
    }
};
// 函数接口
// image2PonitCloud 将rgb图转换为点云
PointCloud::Ptr image2PointCloud(cv::Mat &rgb, cv::Mat &depth, CAMERA_INTRINSIC_PARAMETERS &camera);

// point2dTo3d 将单个点从图像坐标转换为空间坐标
// input: 3维点Point3f (u,v,d)
cv::Point3f point2dTo3d(cv::Point3f &point, CAMERA_INTRINSIC_PARAMETERS &camera);

// computeKeyPointsAndDesp 同时提取关键点与特征描述子
void computeKeyPointsAndDesp(FRAME &frame, string detector, string descriptor);

// estimateMotion 计算两个帧之间的运动
// 输入：帧1和帧2, 相机内参
RESULT_OF_PNP estimateMotion(FRAME &frame1, FRAME &frame2, CAMERA_INTRINSIC_PARAMETERS &camera, ParameterReader &pr);

inline static CAMERA_INTRINSIC_PARAMETERS getDefaultCamera()
{
    ParameterReader pr;
    CAMERA_INTRINSIC_PARAMETERS camera;
    camera.cx = pr.config["camera"]["cx"].as<double>();
    camera.cy = pr.config["camera"]["cy"].as<double>();
    camera.fx = pr.config["camera"]["fx"].as<double>();
    camera.fy = pr.config["camera"]["fy"].as<double>();
    camera.scale = pr.config["camera"]["scale"].as<double>();
    return camera;
}
// cvMat2Eigen
Eigen::Isometry3d cvMat2Eigen(cv::Mat &rvec, cv::Mat &tvec);

// joinPointCloud
PointCloud::Ptr joinPointCloud(PointCloud::Ptr original, FRAME &newFrame, Eigen::Isometry3d T,
                               CAMERA_INTRINSIC_PARAMETERS &camera);

//the following are UBUNTU/LINUX ONLY terminal color
#define RESET "\033[0m"
#define BLACK "\033[30m" /* Black */
#define RED "\033[31m" /* Red */
#define GREEN "\033[32m" /* Green */
#define YELLOW "\033[33m" /* Yellow */
#define BLUE "\033[34m" /* Blue */
#define MAGENTA "\033[35m" /* Magenta */
#define CYAN "\033[36m" /* Cyan */
#define WHITE "\033[37m" /* White */
#define BOLDBLACK "\033[1m\033[30m" /* Bold Black */
#define BOLDRED "\033[1m\033[31m" /* Bold Red */
#define BOLDGREEN "\033[1m\033[32m" /* Bold Green */
#define BOLDYELLOW "\033[1m\033[33m" /* Bold Yellow */
#define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */
#define BOLDMAGENTA "\033[1m\033[35m" /* Bold Magenta */
#define BOLDCYAN "\033[1m\033[36m" /* Bold Cyan */
#define BOLDWHITE "\033[1m\033[37m" /* Bold White */